// #include "Drive.hpp"

// // 参数初始化
// int servoOutput; // 舵机PID计算值
// int servoDutyCycle;
// int motorDutyCycle_L;
// int motorDutyCycle_R;

// int lastDuty = servo_angle_mid;

// const int motor_goal = 56;
// const int dservo = servo_angle_max - servo_angle_min;
// const int k = 1 - abs(servoDutyCycle - servo_angle_mid) / dservo;

// erciPD pd(1.5, 1, 1, 1, 0); // output = Kp * error + Kp2 * abs(error) * error + Kd * deltaError + Kd2 * gyroZ
// cascadePID pd2(1, 0.001, 0.001, 1, 0.001, 0.001, 0.1);
// incrPID pi(1, 0.001, 0.001); // output = Kp * error + Ki * integral + Kd * (error - prevError);

// void Drive(float &angle, int &chujie)
// {
//     // MPU
//     int16_t Gyro_xv = Gyro_value(0);
//     int16_t Gyro_yv = Gyro_value(1);
//     int16_t Gyro_zv = Gyro_value(2);
//     int16_t Gyro_xa = Gyro_value(3);
//     int16_t Gyro_ya = Gyro_value(4);
//     int16_t Gyro_za = Gyro_value(5);

//     // 编码器读取速度
//     float LSpeed = L_encoder_value();
//     float RSpeed = R_encoder_value();
//     cout << "\n Encoder_L:  " << LSpeed << "   Encoder_R: " << RSpeed << endl;

//     int angle_goal_wyz = servo_angle(servoDutyCycle);

//     if (chujie == 1)
//     {
//         Servo_value(146000);
//         LMotor_value(0);
//         RMotor_value(0);
//     }
//     else
//     {
//         servoOutput = pd.update(angle_goal, angle, Gyro_zv);
//         // servoOutput = pd.update(angle , angle_goal_wyz , Gyro_zv);

//         servoDutyCycle = (lastDuty + servoOutput * 100) * 0.86 + lastDuty * 0.14;
//         motorDutyCycle_L = pi.update(motor_goal, LSpeed);
//         motorDutyCycle_R = pi.update(motor_goal, RSpeed);
//     }

//     // 舵机电机输出
//     Servo_value(servoDutyCycle);
//     LMotor_value(motorDutyCycle_L);
//     RMotor_value(motorDutyCycle_R);

//     lastDuty = servoDutyCycle;

//     // 电机方向（不动）
//     LMotor_dir();
//     RMotor_dir();
// }